# Import the necessary libraries
import serial
import os
import sys
import time
import rospy
from sensor_msgs.msg import Imu
from std_msgs.msg import Header
from std_msgs.msg import Int16

# Define a function to convert a four-digit hexadecimal number to a signed integer
def int16_t(num):
    # Check if the integer is greater than or equal to 2^15
    if num >= 2**15:
        # Convert the integer to a signed integer
        signed_int = num - 2**16
    else:
        # The integer is already a signed integer
        signed_int = num
    return signed_int


# Define the serial port and baud rate
ser = serial.Serial(sys.argv[1], 921600)

# Open the serial port
#ser.open()

# Set save path
# save_path = os.path.expanduser(sys.argv[1])
# save_path = os.path.expanduser('/media/huarsuma/2FEA-9429/dataset/test')

# Define the file names to save the data
#encoder_file_name = 'encoder.txt'
#imu_file_name = 'imu.txt'

global ctrl_command
ctrl_command = "AAAF051102013AAC"

# Initialize the ROS node
def command_callback(data):
    global ctrl_command
    ctrl_command = data.frame_id
try:
    rospy.init_node('encoder_node', anonymous=True)
except rospy.exceptions.ROSException as e:
    if str(e) == "rospy.init_node() has already been called with different arguments: ('imu_node', ['serial_data.py', '/home/huarsuma/dataset/test'], True, None, False, False)":
        pass
    else:
        raise e
rospy.Subscriber('command', Header, command_callback)        


try:
    while True:
        # Read the data from the serial port
        data = ser.read(1)

        # Check if the data is the start of a frame
        if data == b'\xaa':
            # Read the second byte
            data += ser.read(1)
    
            # Check if the second byte is the start of a frame
            if data[1] == 0xaf:
                # Read the next 4 bytes
                data += ser.read(3)
    
                # Check if the frame is encoder data    
                if data[2:5] == b'\x01\x01\x02':
                    # Read the next 3 bytes
                    data += ser.read(3)
                    #print(data)
    
                    # Verify the checksum    
                    if sum(data[:-1]) & 0xff == data[-1]:
                        # Extract the encoder data
                        high_byte = data[5]
                        low_byte = data[6]
                        encoder_data = high_byte*256 + low_byte;
                        timestamp = time.time_ns()

                        # Publish the encoder data to ROS topic
                        #global ctrl_command
                        pub = rospy.Publisher('encoder_data', Header, queue_size=10)
                        header = Header()
                        header.stamp = rospy.Time.now()
                        header.seq = encoder_data
                        header.frame_id = ctrl_command
                        #header.frame_id = str(encoder_data)
                        pub.publish(header)
                        
except KeyboardInterrupt:
    print("CTRL C STOP")



